#include "origincar_base/origincar_base.h"
#include "rclcpp/rclcpp.hpp"
#include "origincar_base/Quaternion_Solution.h"
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" 
#include "origincar_msg/msg/data.hpp"
#include <cmath> // 必须包含数学库
#include "std_msgs/msg/int32.hpp"


using std::placeholders::_1;
using namespace std;
void sigintHandler(int sig);
sensor_msgs::msg::Imu Mpu6050;
rclcpp::Node::SharedPtr node_handle = nullptr;


int main(int argc, char *argv[])
{
    // 初始化ROS2客户端库，必须在main函数中调用
    rclcpp::init(argc, argv);
    
    // 注册信号处理函数，当接收到SIGINT信号时调用sigintHandler处理
    signal(SIGINT, sigintHandler);
    
    // 实例化origincar_base对象，创建Robot_Control机器人控制实例
    origincar_base Robot_Control;
    
    // 调用Control方法开始机器人的控制流程
    Robot_Control.Control();
    
    // 关闭ROS2客户端库，必须在main函数中调用以正确退出程序
    rclcpp::shutdown();
    
    // main函数返回0，表示程序正常结束
    return 0;
}

/**
 * @brief 将IMU传感器的高低8位数据转换为16位数据
 * 
 * 本函数的目的是将来自IMU传感器的高低8位数据合并成一个16位的数据值。由于IMU传感器的数据输出是先高字节后低字节，
 * 因此需要将高字节数据左移8位并与低字节数据进行或操作，以得到正确的16位数据值。
 * 
 * @param Data_High 高8位数据
 * @param Data_Low 低8位数据
 * @return short 返回转换后的16位数据
 */
short origincar_base::IMU_Trans(uint8_t Data_High, uint8_t Data_Low)
{
    // 定义一个16位的变量用于存储转换后的数据
    short transition_16;
    // 初始化16位变量为0，确保其初始值不会对数据合并产生影响
    transition_16 = 0;
    // 将高8位数据左移8位，并与低8位数据进行或操作，合并高低字节数据
    transition_16 |= Data_High << 8;
    transition_16 |= Data_Low;
    // 返回转换后的16位数据
    return transition_16;
}

/**
 * @brief 将高低字节转换为浮点数
 * 
 * 本函数的目的是将两个字节的数据（一个高位字节和一个低位字节）合并成一个16位的整数，
 * 然后将这个整数转换为浮点数。这种转换通常用于处理从传感器或其他设备接收到的
 * 分离的高低字节数据，将其转换为原始的浮点数表示。
 * 
 * @param Data_High 高位字节
 * @param Data_Low  低位字节
 * @return float    转换后的浮点数
 */
float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
{
    float data_return;            // 定义用于返回的浮点数变量
    short transition_16;          // 定义一个16位短整型变量，用于高低字节的合并
    transition_16 = 0;            // 初始化16位短整型变量为0，确保其低位清零
    transition_16 |=  Data_High<<8; // 将高位字节左移8位并进行按位或操作，放入高8位
    transition_16 |=  Data_Low;     // 将低位字节进行按位或操作，放入低8位
    data_return   =  (transition_16 / 1000)              // 高8位作为整数部分
                 + (transition_16 % 1000)*0.001;         // 低8位作为小数部分，转换为千分之一的浮点数
    return data_return;          // 返回转换后的浮点数
}

/**
 * @brief 处理Akm指令速度和转向角度的数据，并通过串口发送
 * 
 * 本函数的主要职责是接收一个AckermannDriveStamped消息，将其包含的速度和转向角度
 * 转换为特定格式的数据，并通过串口发送这些数据。这是为了控制车辆的行驶和转向。
 * 
 * @param akm_ctl 指向AckermannDriveStamped消息的智能指针，包含车辆的控制指令
 */
void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
{
    short  transition;
  
    // 初始化发送数据的头部
    Send_Data.tx[0]=FRAME_HEADER;
    Send_Data.tx[1] = 0;
    Send_Data.tx[2] = 0; 

    // 处理速度并将其转换为短数据格式
    transition=0;
    transition = akm_ctl->drive.speed*1000;
    Send_Data.tx[4] = transition;
    Send_Data.tx[3] = transition>>8;

    // 处理转向角度并将其转换为短数据格式
    transition=0;
    transition = akm_ctl->drive.steering_angle*1000/2;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition>>8;

    // 计算并添加校验和和尾部标识
    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
    Send_Data.tx[10]=FRAME_TAIL;

    // 尝试通过串口发送数据，如果发生异常则记录错误
    try {
      Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
    } catch (serial::IOException& e) {
        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
    }
}

/**
 * @brief 处理接收到的命令速度消息，并通过串口发送数据
 * 
 * 本函数的主要职责是将接收到的命令速度消息（线性速度和角速度）转换为特定格式的數據包，
 * 然后通过串口发送这些数据。这是为了控制基础车根据接收到的速度指令进行移动。
 * 
 * @param twist_aux 指向命令速度消息的指针，包含线性速度和角速度信息
 */
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
    short  transition;
    
    // 数据包初始化，设置帧头
    Send_Data.tx[0]=FRAME_HEADER;
    
    // 数据包中的保留字节设置为0
    Send_Data.tx[1] = 0;
    Send_Data.tx[2] = 0; 

    // 处理线性速度x分量，并将其转换为整数后发送
    transition=0;
    transition = twist_aux->linear.x*1000;
    Send_Data.tx[4] = transition;
    Send_Data.tx[3] = transition>>8;

    // 处理线性速度y分量，并将其转换为整数后发送
    transition=0;
    transition = twist_aux->linear.y*1000;
    Send_Data.tx[6] = transition;
    Send_Data.tx[5] = transition>>8;

    // 处理角速度z分量，并将其转换为整数后发送
    transition=0;
    transition = (twist_aux->angular.z)*1000;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition>>8;

    // 计算并添加校验和
    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
    
    // 添加帧尾标志
    Send_Data.tx[10]=FRAME_TAIL;

    try {
      if (akm_cmd_vel == "none") {
        // 尝试通过串口发送数据
        Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
      } 
    } catch (serial::IOException& e) {
        // 如果发生串口异常，记录错误日志
        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
    }
}


/**
 * @brief 发布IMU传感器数据
 * 
 * 本函数负责将IMU(惯性测量单元)传感器的数据打包成ROS2消息，并发布出去
 * 它包含了传感器在某个时间点的定向、角速度和线性加速度数据
 */
void origincar_base::Publish_ImuSensor()
{
    // 创建一个IMU数据消息对象
    sensor_msgs::msg::Imu Imu_Data_Pub;
    
    // 设置消息的头信息，包括数据的时间戳和参考坐标系ID
    Imu_Data_Pub.header.stamp = rclcpp::Node::now();
    Imu_Data_Pub.header.frame_id = gyro_frame_id; 
                                                  
    // 设置IMU的定向数据（四元数表示）
    Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
    Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
    Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
    Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
    
    // 设置定向数据的协方差矩阵，表示对数据不确定性的估计
    Imu_Data_Pub.orientation_covariance[0] = 1e6; 
    Imu_Data_Pub.orientation_covariance[4] = 1e6;
    Imu_Data_Pub.orientation_covariance[8] = 1e-6;
    
    // 设置IMU的角速度数据
    Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
    Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
    Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
    
    // 设置角速度数据的协方差矩阵
    Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
    Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
    Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
    
    // 设置IMU的线性加速度数据
    Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
    Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
    Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;

    // 发布IMU数据消息
    imu_publisher->publish(Imu_Data_Pub);
}


void origincar_base::sign4return_callback(const std_msgs::msg::Int32::SharedPtr msg) 
{
  if (msg->data == 6)
  {
      RCLCPP_INFO(this->get_logger(), "Received signal 6 任务三开始");
      Robot_Pos.X = 2.5;
      Robot_Pos.Y = 2.1;
      Robot_Pos.Z = -M_PI / 2.0;
  }
  if (msg->data == -1)
  {
      RCLCPP_INFO(this->get_logger(), "Received signal 1 任务一开始");
      Robot_Pos.X = 0.7;
      Robot_Pos.Y = 0.2;
      Robot_Pos.Z = 0.0;
  }
}
/**
 * @brief 发布机器人的odom信息
 * 
 * 本函数负责将机器人的位置和速度信息转换并发布为odom消息。它执行以下操作：
 * 1. 计算机器人的四元数表示的朝向。
 * 2. 填充odom消息的头部信息和机器人位置数据。
 * 3. 填充odom消息的线速度和角速度数据。
 * 4. 发布odom消息，以及机器人的位置和速度数据。
 */
void origincar_base::Publish_Odom()
{
    // 初始化一个四元数，并设置其roll, pitch, yaw值为机器人的朝向
    tf2::Quaternion q;
    q.setRPY(0,0,Robot_Pos.Z);
    // 将tf2四元数转换为geometry_msgs消息格式的四元数
    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
    
    // 初始化用于存储机器人位置和速度的数据消息
    origincar_msg::msg::Data robotpose;
    origincar_msg::msg::Data robotvel;
    // 初始化用于发布机器人odom信息的消息
    nav_msgs::msg::Odometry odom;

    // 设置odom消息的头部信息，包括时间戳和参考坐标系
    odom.header.stamp = rclcpp::Node::now();
    odom.header.frame_id = odom_frame_id;
    odom.child_frame_id = robot_frame_id;

    // 填充odom消息的机器人位置数据
    odom.pose.pose.position.x = Robot_Pos.X;
    odom.pose.pose.position.y = Robot_Pos.Y;
    // 由于机器人在平面内运动，z轴位置设为0
    odom.pose.pose.position.z = 0.0;
    // 设置odom消息的机器人的朝向四元数
    odom.pose.pose.orientation = odom_quat;

    // 填充odom消息的机器人速度数据
    odom.twist.twist.linear.x =  Robot_Vel.X;
    odom.twist.twist.linear.y =  Robot_Vel.Y;
    // 由于机器人在平面内运动，只考虑Z轴的角速度
    odom.twist.twist.angular.z = Robot_Vel.Z; 

    // 填充机器人位置数据消息
    robotpose.x = Robot_Pos.X;
    robotpose.y = Robot_Pos.Y;
    robotpose.z = Robot_Pos.Z;

    // 填充机器人速度数据消息
    robotvel.x = Robot_Vel.X;
    robotvel.y = Robot_Vel.Y;
    robotvel.z = Robot_Vel.Z;

    // 发布odom消息，以及机器人位置和速度数据消息
    odom_publisher->publish(odom);
    robotpose_publisher->publish(robotpose);
    robotvel_publisher->publish(robotvel); 
}

/**
 * @brief 发布电压信息
 * 
 * 本函数负责周期性地发布汽车的电压信息。它使用一个静态变量Count_Voltage_Pub来计数，当计数超过10时，
 * 将Power_voltage的值封装到std_msgs::msg::Float32消息中，并通过voltage_publisher发布器发布。
 * 这种发布机制确保了电压信息不会被过于频繁地发送，而是以一种节制的方式进行。
*/
void origincar_base::Publish_Voltage()
{
    // 创建一个Float32消息对象，用于承载电压数据
    std_msgs::msg::Float32 voltage_msgs;
    
    // 定义一个静态浮点数变量Count_Voltage_Pub，用于计数电压发布次数
    // 当Count_Voltage_Pub超过10时，重置其值为0，并发布电压信息
    int Count_Voltage_Pub = 0;
    if (Count_Voltage_Pub++ > 10) {
        Count_Voltage_Pub = 0;
        
        // 设置电压消息的数据字段为Power_voltage的值
        voltage_msgs.data = Power_voltage;
        
        // 通过voltage_publisher发布器发布电压消息
        voltage_publisher->publish(voltage_msgs);
    }
}



/**
 * 计算校验和
 * 
 * @param Count_Number 要参与校验的数据数量
 * @param mode 校验模式，0表示对接收的数据校验，1表示对发送的数据校验
 * 
 * @return 返回计算得到的校验和
 * 
 * 此函数根据指定的校验模式，计算并返回数据的校验和校验和用于数据传输的错误检测，
 * 通过异或运算对一组数据进行校验，异或结果全0表示数据无冲突
 */
unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode)
{
    unsigned char check_sum = 0, k;

    // 根据校验模式选择参与校验的数据集
    if (mode == 0) {
      // 对接收的数据进行校验
      for(k=0; k < Count_Number; k++) {
        check_sum = check_sum^Receive_Data.rx[k];
      }
    } else if (mode == 1) {
      // 对发送的数据进行校验
      for (k=0; k < Count_Number; k++) {
        check_sum = check_sum^Send_Data.tx[k];
      }
    }

    // 返回校验和
    return check_sum;
}

// 从原始汽车底盘获取传感器数据
bool origincar_base::Get_Sensor_Data()
{
    // 初始化变量
    short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
    uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; 

    // 从串口读取数据
    Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); 

    // 寻找数据帧的头部和尾部位置
    for (j = 0; j < 24; j++) {
      if (Receive_Data_Pr[j] == FRAME_HEADER)
      Header_Pos=j;
      else if (Receive_Data_Pr[j] == FRAME_TAIL)
      Tail_Pos = j;
    }

    // 根据头部和尾部位置，判断数据帧的完整性并进行相应处理
    if (Tail_Pos == (Header_Pos + 23)) {
      memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
    }  else if (Header_Pos == (1 + Tail_Pos)) {
      for (j = 0;j < 24; j++)
      Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
    }  else {
    return false;
    }

    // 提取数据帧的头部和尾部标识
    Receive_Data.Frame_Header = Receive_Data.rx[0];
    Receive_Data.Frame_Tail = Receive_Data.rx[23];

    // 验证数据帧的完整性和校验和
    if (Receive_Data.Frame_Header == FRAME_HEADER) {
      if (Receive_Data.Frame_Tail == FRAME_TAIL) {
        if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
          // 解析数据帧中的机器人速度和MPU6050数据
          Receive_Data.Flag_Stop=Receive_Data.rx[1];
          Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
          Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
          //修改Robot_Vel.Z
          Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); 

          Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
          Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
          Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
          Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
          Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
          Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);

          // 转换并设置线性加速度和角速度数据
          Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
          Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
          Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;

          Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
          Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
          
          
          float raw_z = Mpu6050.angular_velocity.z =  Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;
            if (!is_calibrated_) {
              if (calib_count_ < 100) {
                  gyro_z_sum_ += raw_z;
                  calib_count_++;
                  
                  // 完成100次采样后计算偏移量
                  if (calib_count_ == 100) {
                      gyro_z_offset_ = gyro_z_sum_ / 100.0;
                      is_calibrated_ = true;
                      RCLCPP_INFO(this->get_logger(), 
                          "Gyro Z-axis calibrated. Offset: %.4f rad/s", gyro_z_offset_);
                  }
                  
                  // 校准阶段输出0
                  Mpu6050.angular_velocity.z = 0.0;
                  Robot_Vel.Z = 0.0;
              }
          } else {
              // 正常使用阶段：应用校准
              Mpu6050.angular_velocity.z = raw_z - gyro_z_offset_;
              Robot_Vel.Z = Mpu6050.angular_velocity.z;
          }
        
          // 计算和设置电源电压
          transition_16 = 0;
          transition_16 |=  Receive_Data.rx[20]<<8;
          transition_16 |=  Receive_Data.rx[21];
          Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;

          return true;
        }
      }
    }

    return false;
}

// 控制函数，用于实时更新机器人的位置和姿态，并发布相关传感器数据
void origincar_base::Control()
{
    // 初始化当前时间和上次时间变量
    rclcpp::Time current_time, last_time;
    // 设置当前时间和上次时间初始值为当前系统时间
    current_time = rclcpp::Node::now();
    last_time = rclcpp::Node::now();
    
    // 循环在ROS系统正常运行期间持续执行
    while(rclcpp::ok()) {
        // 更新当前时间为当前系统时间
        current_time = rclcpp::Node::now();
        // 计算采样时间间隔
        Sampling_Time = (current_time - last_time).seconds();
        
        // 如果成功获取传感器数据
        if (true == Get_Sensor_Data()) {
            // 更新机器人位置，根据速度和采样时间进行积分
            Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) )* Sampling_Time;
            Robot_Pos.Y+=1.13*(Robot_Vel.X * sin(Robot_Pos.Z) )* Sampling_Time;
            if (Robot_Vel.X != 0){
              Robot_Pos.Z+= Robot_Vel.Z * Sampling_Time;
            }

            Robot_Pos.Z = fmod(Robot_Pos.Z + M_PI, 2 * M_PI) - M_PI;//将角度限制在-pi到pi之间

            
            // 根据MPU6050传感器数据更新四元数解决方案
            Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
                  Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
            // 发布IMU传感器数据
            Publish_ImuSensor();
            // 发布电压数据
            Publish_Voltage();
            // 发布里程计数据
            Publish_Odom();
            // 处理节点中的消息，保持节点活跃
            rclcpp::spin_some(this->get_node_base_interface());
        }
        // 更新上次时间为当前时间，为下一次循环做准备
        last_time = current_time;
    }
}

// 构造函数用于初始化origincar_base节点
origincar_base::origincar_base()
: rclcpp::Node ("origincar_base")
{
  // 初始化校准变量
  calib_count_ = 0;
  gyro_z_sum_ = 0.0;
  gyro_z_offset_ = 0.0;
  is_calibrated_ = false;

  // 初始化机器人位置、速度、接收数据、发送数据和MPU6050数据结构体
  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
  memset(&Receive_Data, 0, sizeof(Receive_Data));
  memset(&Send_Data, 0, sizeof(Send_Data));
  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));

  // 设置串口波特率
  int serial_baud_rate = 115200;

  // 声明参数，包括串口名称、命令速度主题名、里程计帧ID等
  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
  this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
  this->declare_parameter<std::string>("odom_frame_id", "odom");
  this->declare_parameter<std::string>("robot_frame_id", "base_link");
  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");

  // 获取参数值
  this->get_parameter("serial_baud_rate", serial_baud_rate);
  this->get_parameter("usart_port_name", usart_port_name);
  this->get_parameter("cmd_vel", cmd_vel);
  this->get_parameter("akm_cmd_vel", akm_cmd_vel);
  this->get_parameter("odom_frame_id", odom_frame_id);
  this->get_parameter("robot_frame_id", robot_frame_id);
  this->get_parameter("gyro_frame_id", gyro_frame_id);

  // 创建发布者，用于发布里程计、IMU数据、电压、机器人姿态和速度信息
  odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
  imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);
  voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);
  robotpose_publisher = create_publisher<origincar_msg::msg::Data>("robotpose", 5);
  robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);

  // 创建TF广播器
  tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);


  //  auto qos_profile = rclcpp::QoS(1)     // 深度设置为1 - 只保留最新消息
  //       .keep_last(1)                     // 历史策略: 保留最后一条
  //       .best_effort()                    // 可靠性策略: BEST_EFFORT (降低延迟)
  //       .durability_volatile()            // 持久性策略: VOLATILE (不需要持久化)
  //       .deadline(std::chrono::milliseconds(100)); // 设置100ms的截止期限(监控用)


  // 创建订阅者，用于订阅命令速度和Ackermann命令速度信息
  Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
      cmd_vel,3, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1));
  Akm_Cmd_Vel_Sub = create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
      akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
  // 创建订阅者，用于接收来自"sign_switch"话题的消息
  sign4return_sub_ = create_subscription<std_msgs::msg::Int32>("/sign4return", 10,std::bind(&origincar_base::sign4return_callback, this, _1));


  // 尝试打开并配置串口
  try  {
    Stm32_Serial.setPort("/dev/ttyACM0");
    Stm32_Serial.setBaudrate(serial_baud_rate);
    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
    Stm32_Serial.setTimeout(_time);
    Stm32_Serial.open();
  } catch (serial::IOException& e) {
    // 如果无法打开串口，输出错误信息
    RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! ");
  }
  // 如果串口成功打开，输出确认信息
  if(Stm32_Serial.isOpen()) {
    RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened");
  }
}


/**
 * @brief SIGINT信号处理函数
 * 
 * 该函数作为信号处理程序，用于在接收到SIGINT信号时执行特定的操作。
 * 在本场景中，当用户中断程序或程序遇到特定信号时，该函数会被调用，
 * 以优雅地关闭OriginBot，并向连接的串口设备发送关闭信号。
 * 
 * @param sig 接收到的信号编号
 */
void sigintHandler(int sig)
{
    sig = sig;
    // 输出中断处理信息，表明程序开始关闭
    printf("OriginBot shutdown...\n");

    // 初始化串口对象，用于与STM32微控制器通信
    serial::Serial Stm32_Serial;
    // 设置串口设备的路径
    Stm32_Serial.setPort("/dev/ttyACM0");
    // 设置串口的波特率
    Stm32_Serial.setBaudrate(115200);
    // 设置串口的超时设置
    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
    Stm32_Serial.setTimeout(_time);
    // 打开串口连接
    Stm32_Serial.open();                                       

    // 初始化发送数据结构体
    SEND_DATA Send_Data;

    // 检查串口是否成功打开
    if (Stm32_Serial.isOpen()) {
        // 填充发送数据的头部和命令字节
        Send_Data.tx[0]=FRAME_HEADER;
        Send_Data.tx[1] = 0;
        Send_Data.tx[2] = 0;

        // 填充发送数据的其他部分，根据上下文，这里可能需要进一步的逻辑处理
        Send_Data.tx[4] = 0;
        Send_Data.tx[3] = 0;

        Send_Data.tx[6] = 0;
        Send_Data.tx[5] = 0;

        Send_Data.tx[7] = 0;
        Send_Data.tx[8] = 0;

        // 计算校验和
        int check_sum = 0;
        for (int k = 0; k < 9; k++) {
            check_sum = check_sum^Send_Data.tx[k];
        }
        // 将校验和添加到发送数据中
        Send_Data.tx[9]=check_sum;
        Send_Data.tx[10]=FRAME_TAIL;

        // 尝试向串口写入数据
        try {
            Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
        } catch (serial::IOException& e) {
            // 捕获并处理串口异常，此处未做具体处理是可能因为异常处理在更高层进行
        }
    }

    // 关闭ROS2接口，清除资源
    rclcpp::shutdown();
}

// 析构函数：origincar_base的资源清理
origincar_base::~origincar_base()
{
  // 输出关闭信息到日志
  RCLCPP_INFO(this->get_logger(),"Shutting down");
}